/***************************************************************************************************************************
 * * state_from_tello.h
 * 【注意】：通过阅读TELLO_NODE有如下发现：
 * a.   imu和odom有部分数据相同：
 *          odom_msg.pose.pose.orientation和imu_msg.orientation均为data.imu.q？（四元数表示的姿态）
 *          odom_msg.twist.twist.angular和imu_msg.angular_velocitydata.imu.gyro_？（三轴角速度）
 * b.   暂时只有里程计可能存在位置（还需验证）
 * 
 * 作者：蓝蓝蓝笨笨
 * 
 * 功能：订阅来自TELLO_NODE中的（与状态相关）话题
 * 1.   订阅odom里程计，包含【位置】，【姿态】，【线速度】，【角速度】
 * 2.   订阅imu惯性导航，包含【姿态】，【角速度】，【角加速度】。
 * 【注意】：话题发布频率
 * rospy和roscpp的publisher中       queue_size 表示消息队列  
 * 根据roswiki介绍该值如下：
 * 省略：会打印警告【同步处理】
 * None：会被阻塞【不建议】
 * 0：无限队列，但是会无限的增加内存消耗
 * 1，2，3：消息速度为10hz时，不会有任何问题
 * 10以上：用于高速率小队列
 * 在TELLO_NODE中设置为1，本程序目前设置为10
***************************************************************************************************************************/
#ifndef STATE_FROM_ROTORS_H
#define STATE_FROM_ROTORS_H

#include <ros/ros.h>
#include <ros/console.h>
#include <bitset>
#include <math_utils.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
#include <uav_drone_msgs/DroneState.h>
#include <uav_drone_msgs/AttitudeReference.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Float64.h>
#include <tf2_msgs/TFMessage.h>
#include <tf/transform_listener.h>

using namespace std;

class state_from_tello
{
    public:
    //变量声明 
    uav_drone_msgs::DroneState _DroneState;
    string mav_name;
    state_from_tello(void):
    state_nh("~")
    {
        // 【订阅】无人机当前位置 坐标系:
        //odom_sub = state_nh.subscribe(std::string("/"+mav_name+"/tello/odom").c_str(), 10, &state_from_rotors::odom_cb,this);
        odom_sub = state_nh.subscribe<nav_msgs::Odometry>("/tello/odom", 10, &state_from_tello::odom_cb,this); 
        // 【订阅】无人机当前欧拉角 坐标系:ENU系（IMU输出频率可在rqt中查看）
        imu_sub = state_nh.subscribe<sensor_msgs::Imu>("/tello/imu", 10, &state_from_tello::imu_cb,this); 
    }
    
    private:

        ros::NodeHandle state_nh;

        ros::Subscriber odom_sub;
        ros::Subscriber imu_sub;

        void odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
        {
            //三轴位置（odom的pose.pose.position不确定是不是位置）
            //实际测试中，odom的X实际是Z（高度，起飞后读值为-150）
            //后续进行相机估计时，高度可用，可将XY按照机体系转换为(0，0)
            _DroneState.position[0] = msg->pose.pose.position.x;
            _DroneState.position[1] = msg->pose.pose.position.y;
            _DroneState.position[2] = msg->pose.pose.position.z;
            //三轴线速度
            _DroneState.velocity[0] = msg->twist.twist.linear.x;
            _DroneState.velocity[1] = msg->twist.twist.linear.y;
            _DroneState.velocity[2] = msg->twist.twist.linear.z;
            /******************************     说明       ***************************
            *   因为四元数和角速度的数据实际与IMU重合，所以不额外进行读取
            ****************************************************************** */
        }
        void imu_cb(const sensor_msgs::Imu::ConstPtr& msg)
        {
            Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
            //Transform the Quaternion to euler Angles
            Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu);
            
            _DroneState.attitude_q.w = q_fcu.w();
            _DroneState.attitude_q.x = q_fcu.x();
            _DroneState.attitude_q.y = q_fcu.y();
            _DroneState.attitude_q.z = q_fcu.z();
            //[0]:  yaw              [1]:   pitch                [2]:    roll
            _DroneState.attitude[0] = euler_fcu[0];
            _DroneState.attitude[1] = euler_fcu[1];
            _DroneState.attitude[2] = euler_fcu[2];

            _DroneState.attitude_rate[0] = msg->angular_velocity.x;
            _DroneState.attitude_rate[1] = msg->angular_velocity.y;
            _DroneState.attitude_rate[2] = msg->angular_velocity.z;

        }

};

    
#endif
